#pragma once

#ifndef _STATE_H_
#define _STATE_H_

#include "common_include.h"
#include"utility.h"
#include"S2.h"

namespace esikf_slam{

struct State{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<State> Ptr;
    static const int DIM_OF_STATE_ = 18;
    // static const int DIM_OF_STATE_ = 17;  //gravity is S2 mainflold
    static const int DIM_OF_NOISE_ = 12;
    static const int IDX_pos_ = 0;
    static const int IDX_vel_ = 3;
    static const int IDX_q_ = 6;
    static const int IDX_bg_ = 9;
    static const int IDX_ba_ = 12;
    static const int IDX_gra_ = 15;

    // !@State 
    V3D pos_;   // position in world frame
    V3D vel_;   // velocity in world frame
    Q4D q_;  // rotation from world frame
    V3D bg_;   // gyroscope bias
    V3D ba_;   // acceleartion bias
    V3D gra_;   // gravity world frame
    // S2 gra_;

    Eigen::Matrix<double,DIM_OF_STATE_,DIM_OF_STATE_> cov_P_;

    State(){
        reset();
    }
  
    void reset(){
        pos_.setZero();
        vel_.setZero();
        q_.setIdentity();
        ba_.setZero();
        bg_.setZero();
        gra_ << 0.0, 0.0, -G0_;
        // gra_.reset();
        cov_P_.setZero();
    }

  State& operator=(const State& other) {
    if (this == &other) return *this;

    this->pos_ = other.pos_;
    this->vel_ = other.vel_;
    this->q_ = other.q_;
    this->bg_ = other.bg_;
    this->ba_ = other.ba_;
    this->gra_ = other.gra_;
    this->cov_P_ = other.cov_P_;

    return *this;
  }

  // boxMinus operator
  void boxMinus(const State& stateIn,
                Eigen::Matrix<double, DIM_OF_STATE_, 1>& xk) {
    xk.template segment<3>(IDX_pos_) = this->pos_ - stateIn.pos_;
    xk.template segment<3>(IDX_vel_) = this->vel_ - stateIn.vel_;
    SO3 delta_rot =(stateIn.q_.inverse()*this->q_).toRotationMatrix();
    xk.template segment<3>(IDX_q_) = delta_rot.log();
    xk.template segment<3>(IDX_bg_) = this->bg_ - stateIn.bg_;
    xk.template segment<3>(IDX_ba_) = this->ba_ - stateIn.ba_;
    xk.template segment<3>(IDX_gra_) = this->gra_ - stateIn.gra_;
    // this->gra_.boxminus(stateIn.gra_,xk.template segment<2>(IDX_gra_));
  }

    // boxPlus operator
  void boxPlus(const Eigen::Matrix<double, DIM_OF_STATE_, 1>& xk,
               State& stateOut) {
    stateOut.pos_ = this->pos_ + xk.template segment<3>(IDX_pos_);
    stateOut.vel_ = this->vel_ + xk.template segment<3>(IDX_vel_);
    stateOut.q_ = this->q_.toRotationMatrix()*Sophus::SO3d::exp(xk.template segment<3>(IDX_q_)).matrix();
    stateOut.q_ = stateOut.q_.normalized();
    stateOut.bg_ = this->bg_ + xk.template segment<3>(IDX_bg_);
    stateOut.ba_ = ba_ + xk.template segment<3>(IDX_ba_);
    stateOut.gra_ = gra_ + xk.template segment<3>(IDX_gra_);
    // gra_.boxplus(xk.template segment<2>(IDX_gra_));
  }

};



}
#endif  
